import Astar_improve as Astar_improved
import Astar_no_improve as Astar_no_improved
import visualize
import numpy as np
import pygame
import point_prune

# 节点是否可行由路规算法判断
start = (0, 0)
end = (20, 47)


def my_astar():
    new_map = np.load('npy/resized_map.npy')
    node_matrix = Astar_improved.create_node_matrix(new_map)
    astar = Astar_improved.AstarPlanner(node_matrix, start, end, new_map)
    path = astar.planning()

    grid = visualize.create_map_grid(new_map)
    processed = astar.processed_grid
    visualize.draw_astar(grid, path, processed)

    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()


# 经典A*小地图
def classic_astar():
    new_map = np.load('npy/resized_map.npy')
    node_matrix = Astar_no_improved.create_node_matrix(new_map)
    astar = Astar_no_improved.AstarPlanner(node_matrix, start, end, new_map)
    path = astar.planning()

    grid = visualize.create_map_grid(new_map)
    processed = astar.processed_grid
    visualize.draw_astar(grid, path, processed)

    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()


# 经典A* 但是用的是更大的地图
def raw_astar():
    reload_flag = False
    visualize.set_visual_col_row(400, 400, 1)
    new_map = np.load('npy/resized_map_400.npy')
    node_matrix = Astar_no_improved.create_node_matrix(new_map)
    if reload_flag:
        start = (0, 0)
        end = (250, 200)
        astar = Astar_no_improved.AstarPlanner(node_matrix, start, end, new_map)
        path = astar.planning()
        processed = astar.processed_grid
        np.save('npy/raw_path_400.npy', path)
    else:
        path = np.load('npy/raw_path_400.npy')
        processed = []

    breaks, lens, dirs = point_prune.breakpoint_detection(path)
    deleted = point_prune.delete_dot(breaks, new_map)

    grid = visualize.create_map_grid('npy/resized_map_400.npy')

    visualize.draw_astar(grid, path, processed)
    for dot in deleted:
        x = dot[0] + 1
        y = dot[1] + 1
        grid[x][y].show((250, 0, 255), 0)

    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()


def tmp_func_correct():
    new_map = np.load('npy/resized_map.npy')
    grid = visualize.create_map_grid(new_map)
    path_tmp = [
        [20, 20, 20, 20, 20, 19, 18, 17, 16, 16, 16, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 7, 7, 7, 7, 7, 7, 7, 6, 5, 4,
         4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 5, 5, 5, 5, 4, 3, 2, 1, 0]
        ,
        [47, 46, 45, 44, 43, 42, 41, 40, 39, 38, 37, 36, 35, 34, 33, 33, 32, 32, 31, 30, 29, 28, 27, 26, 25, 24, 23, 22,
         21, 20, 19, 18, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0]]

    path = np.transpose(np.array(path_tmp))
    breaks, lens, dirs = point_prune.breakpoint_detection(path)
    deleted = point_prune.delete_dot(breaks, new_map)

    # 颜色
    red = (255, 0, 0)
    yellow = (255, 255, 0)
    visualize.draw_astar(grid, path, [])
    for item in breaks:
        i = item[0]
        j = item[1]
        grid[i+1][j+1].show(yellow, 0)

    for item in deleted:
        i = item[0]
        j = item[1]
        grid[i+1][j+1].show(red, 0)
        print([i,j])
    # 保持窗口打开直到点击结束
    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()


def paper_compare():
    # 颜色
    blue = (0, 0, 255)
    red = (255, 0, 0)
    yellow = (255, 255, 0)

    map = np.load('npy/resized_map_600_600.npy')
    visualize.set_param(600, 600, 4)
    grid = visualize.create_map_grid(map)
    # grid[21][21].show(red, 0)
    # grid[51][71].show(red, 0)

    # node_matrix = Astar_improved.create_node_matrix(map)
    # astar = Astar_improved.AstarPlanner(node_matrix, (20,20), (50,70), map)

    # node_matrix = Astar_no_improved.create_node_matrix(map)
    # astar = Astar_no_improved.AstarPlanner(node_matrix, (20,20), (50,70), map)
    #
    # path = astar.planning()
    # processed = astar.processed_grid
    # np.save('txt_data/path_60_80_no.npy', path)
    # np.save('txt_data/processed_60_80_no.npy', processed)
    # visualize.draw_astar(grid, path, processed)

    # path = np.load('txt_data/path_60_80_improved.npy')
    # breaks, lens, dirs = point_prune.breakpoint_detection(path)
    # deleted = point_prune.delete_dot(breaks, map)
    # print("break:\n", breaks)
    # print("deleted:\n", deleted)
    # for item in path:
    #     x = item[0] + 1
    #     y = item[1] + 1
    #     grid[x][y].show(blue, 0)
    #
    # for item in breaks:
    #     i = item[0]
    #     j = item[1]
    #     grid[i+1][j+1].show(yellow, 0)
    #
    # for item in deleted:
    #     x = item[0] + 1
    #     y = item[1] + 1
    #     grid[x][y].show(red, 0)


    running = True
    while running:
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                running = False
    pygame.quit()



if __name__ == '__main__':
    # my_astar()
    # my_astar()
    # path = np.load('npy/raw_path.npy')
    # print(path)
    # classic_astar()
    # tmp_func_correct()
    # dot1=(10,32)
    # dot2=(16,36)
    # map_grid_ = np.load('npy/resized_map.npy')
    # flag = point_prune.is_crash(dot1, dot2, map_grid=map_grid_)
    # print(flag)
    paper_compare()
